#include "ros/ros.h"
#include "tf/tf.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TwistStamped.h"

tf2_ros::Buffer buffer;

int main(int argc, char **argv){
    setlocale(LC_CTYPE, "zh_CN.utf8");
    ros::init(argc, argv, "puresuit");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");
    tf2_ros::TransformListener listener(buffer);
    geometry_msgs::TransformStamped ts = buffer.lookupTransform("base_link", "imu", ros::Time(0), ros::Duration(1.0));
    double x,y,z,qx,qy,qz,qw;
    x = ts.transform.translation.x;
    y = ts.transform.translation.y;
    z = ts.transform.translation.z;
    qx = ts.transform.rotation.x;
    qy = ts.transform.rotation.y;
    qz = ts.transform.rotation.z;
    qw = ts.transform.rotation.w;
    ROS_INFO("四元数 %lf %lf %lf %lf %lf %lf %lf", x, y, z, qx, qy, qz, qw);
    double roll, pitch, yaw;
    tf::Quaternion q;
    tf::quaternionMsgToTF(ts.transform.rotation, q);
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
    ROS_INFO("欧拉角 %lf %lf %lf %lf %lf %lf", x, y, z, roll, pitch, yaw);
    ros::spin();
    return 0;
}
